// $Id: ao.c 487 2014-05-02 23:41:32Z alan.watson.f $

// This file is part of the control system for the Oaxaca instrument.

////////////////////////////////////////////////////////////////////////////////

#define _POSIX_C_SOURCE 200112L

#include <float.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include <sys/time.h>

#include "oaxaca-config.h"
#include "oaxaca.h"
#include "ao.h"
#include "array.h"
#include "dimm.h"
#include "spots.h"
#include "telescope.h"
#include "minmax.h"

static double a0[OAXACA_NA_MAX];
static double a1[OAXACA_NA_MAX];
static double a2[OAXACA_NA_MAX];

static double da0[OAXACA_NA_MAX];
static double da1[OAXACA_NA_MAX];
static double da2[OAXACA_NA_MAX];

static double ds0[OAXACA_NS_MAX];
static double ds1[OAXACA_NS_MAX];
static double ds2[OAXACA_NS_MAX];

static int na = OAXACA_NA_MAX;

static double asaturated[OAXACA_NA_MAX];

void
oaxaca_ao_clear(int index)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];
  
  for (int i = 0; i < OAXACA_NS_MAX; ++i) {
    detector->dsmean[i]  = 0.0;
    detector->dssigma[i] = 0.0;
  }
  detector->rmsdsmean  = 0.0;
  detector->rmsdssigma = 0.0;

  for (int i = 0; i < OAXACA_NS_MAX; ++i)
    ds0[i] = ds1[i] = ds2[i] = 0.0;

  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    da0[i] = da1[i] = da2[i] = 0.0;

  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    a0[i] = a1[i] = a2[i] = 0.0;

  for (int i = 0; i < OAXACA_NA_MAX; ++i)
    asaturated[i] = 0.0;
    
  detector->totaldelay = 0.0;
  oaxaca_shm->wfc.totaldelay = 0.0;
  
  oaxaca_dimm_start(index);
}

void
oaxaca_ao_frame_call_back(int index, array_short_t *frame)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  double ds[OAXACA_NS_MAX];

  int ns = detector->ns;

  double r[OAXACA_NS_MAX];
  for (int i = 0; i < ns; ++i)
    r[i] = detector->r[i];

  double s[OAXACA_NS_MAX];
  oaxaca_spots_refine(frame, ns, r, detector->searchradius, detector->refinementradius, s);
  
  for (int i = 0; i < ns; ++i)
    detector->s[i] = s[i];

  for (int i = 0; i < ns; ++i)
    ds[i] = s[i] - r[i];

  for (int i = 0; i < ns; ++i) {
    ds0[i] += 1.0;
    ds1[i] += ds[i];
    ds2[i] += ds[i] * ds[i];
  }
  
  oaxaca_dimm_next(index);

  double da[na];
  for (int i = 0; i < na; ++i) {
    da[i] = 0.0;
    for (int j = 0; j < ns; ++j)
      da[i] += detector->c_matrix[i][j] * ds[j];
    da[i] *= detector->g[i];
  }
  
  for (int i = 0; i < na; ++i) {
    double a = oaxaca_shm->wfc.a[i];
    a -= da[i];
    if (a != a)
      a = 0.0;
    else if (a <= -1.0) {
      a = -1.0;
      asaturated[i]++;
    } else if (a >= +1.0) {
      a = +1.0;
      asaturated[i]++;
    }
    oaxaca_shm->wfc.a[i] = a;
  }
  
  struct timeval tv;
  gettimeofday(&tv, NULL);
  double timestamp = tv.tv_sec + tv.tv_usec * 1e-6;

  detector->totaldelay += timestamp - detector->timestamp;
  oaxaca_shm->wfc.timestamp = timestamp;

#if 0

  // This code is an experiment to determine the WFC response delay.
  
  static FILE *testfp;
  static double sref[OAXACA_NS_MAX];
  if (detector->frame == 50) {
    testfp = fopen("/tmp/test.csv", "w");
  }
  if (50 <= detector->frame && detector->frame <= 150) {
    fprintf(testfp, "%lu", detector->frame);
    for (int i = 0; i < ns; ++i)
      fprintf(testfp, ",%.2f", s[i] - sref[i]);
    fprintf(testfp, "\n");
    for (int i = 0; i < ns; i += 2)
      oaxaca_info("test: frame %lu spot %d (%+.2f,%+.2f).", detector->frame, i / 2, s[i] - sref[i], s[i + 1] - sref[i + 1]);
  }
  if (detector->frame == 150)
    fclose(testfp);
  for (int i = 0; i < ns; ++i)
    sref[i] = s[i];

  if (detector->frame == 0) {
    oaxaca_info("test: setting WFC to zero.");
    for (int i = 0; i < na; ++i)
      oaxaca_shm->wfc.a[i] = 0;
    oaxaca_shm->wfc.command = 't';
  } else if (detector->frame == 100) {
    oaxaca_info("test: moving WFC.");
    oaxaca_shm->wfc.a[0] = -0.1;		// For TM
    //oaxaca_shm->wfc.a[1] = 1.0;		// For DM
    oaxaca_shm->wfc.command = 't';
  }

 #else
 
   // This code is for normal correction.
 
  oaxaca_shm->wfc.command = 't';
 
 #endif

  for (int i = 0; i < na; ++i) {
    da0[i] += 1.0;
    da1[i] += da[i];
    da2[i] += da[i] * da[i];
  }

  for (int i = 0; i < na; ++i) {
    double a = oaxaca_shm->wfc.a[i];
    a0[i] += 1.0;
    a1[i] += a;
    a2[i] += a * a;
  }

  int interval = 5;
  unsigned long nframe = interval * detector->framefrequency;
  if (detector->frame != 0 && detector->frame % nframe == 0) {
  
    for (int i = 0; i < na; ++i) {
      double mean = a1[i] / a0[i];
      double variance = (a0[i] * a2[i] - a1[i] * a1[i]) / a0[i] / (a0[i] - 1);
      double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
      oaxaca_shm->wfc.amean[i]  = mean;
      oaxaca_shm->wfc.asigma[i] = sigma;
    }
 
    for (int i = 0; i < na; ++i) {
      double mean = da1[i] / da0[i];
      double variance = (da0[i] * da2[i] - da1[i] * da1[i]) / da0[i] / (da0[i] - 1);
      double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
      oaxaca_shm->wfc.damean[i]  = mean;
      oaxaca_shm->wfc.dasigma[i] = sigma;
    }
    
    for (int i = 0; i < na; ++i)
      oaxaca_shm->wfc.asaturated[i] = asaturated[i] / a0[i];
 
    for (int i = 0; i < OAXACA_NA_MAX; ++i)
      da0[i] = da1[i] = da2[i] = 0.0;
    for (int i = 0; i < OAXACA_NA_MAX; ++i)
      a0[i] = a1[i] = a2[i] = 0.0;
    for (int i = 0; i < OAXACA_NA_MAX; ++i)
      asaturated[i] = 0.0;
      
  }

  oaxaca_telescope_next(index);

}

static void
ao_log(int index)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  FILE *fp = fopen("ao.csv", "a");
  if (fp == NULL)
    oaxaca_error("ao: unable to open log file.");

  time_t seconds = time(NULL);
  struct tm tm = *gmtime(&seconds);
  char yyyymmdd[] = "YYYY-MM-DD";
  strftime(yyyymmdd, sizeof(yyyymmdd), "%Y-%m-%d", &tm);
  char hhmmss[] = "hh:mm:ss";
  strftime(hhmmss, sizeof(hhmmss), "%H:%M:%S", &tm);
  fprintf(fp, "\"%s %s\"", yyyymmdd, hhmmss);
  fprintf(fp, ",\"%s\"", yyyymmdd);
  fprintf(fp, ",\"%s\"", hhmmss);

  fprintf(fp, ",%.3f", detector->hartmannfwhm);
  fprintf(fp, ",%.3e", detector->totaldelay);
  fprintf(fp, ",%.3e", oaxaca_shm->wfc.totaldelay);
  fprintf(fp, "\n");
  
  fclose(fp);  
}

void
oaxaca_ao_integration_call_back(int index, array_short_t *mean, const char *basename)
{
  volatile oaxaca_detector_t *detector = &oaxaca_shm->detector[index];

  int ns = detector->ns;

  {
    double meansumsq = 0.0;
    double sigmasumsq = 0.0;
    for (int i = 0; i < ns; ++i) {
      double mean = ds1[i] / ds0[i];
      double variance = (ds0[i] * ds2[i] - ds1[i] * ds1[i]) / ds0[i] / (ds0[i] - 1.0);
      double sigma = (variance >= 0.0) ? sqrt(variance) : 0.0;
      detector->dsmean[i]  = mean;
      detector->dssigma[i] = sigma;
      //oaxaca_info("dsmean[%d] = %+.2f dssigma[%d] = %.2f", i, mean, i, sigma);
      meansumsq += mean * mean;
      sigmasumsq += sigma * sigma;
    }
    double rmsdsmean = sqrt(meansumsq / ns);
    double rmsdssigma = sqrt(sigmasumsq / ns);
    detector->rmsdsmean = rmsdsmean;
    detector->rmsdssigma = rmsdssigma;
    oaxaca_debug("ao: rmsdssigma = %.3f pixels (raw)", rmsdssigma);
    double rmsdssigmanoise = 57.294 * pow(detector->eshm, -0.8748);
    oaxaca_debug("ao: rmsdssigma = %.3f pixels (noise)", rmsdssigmanoise);
    double rmsdssigmacorrected = 
      (rmsdssigmanoise > rmsdssigma) ? 0.0 : sqrt(rmsdssigma * rmsdssigma - rmsdssigmanoise * rmsdssigmanoise);
    oaxaca_debug("ao: rmsdssigma = %.3f pixels (corrected)", rmsdssigmacorrected);
    double hartmannfwhm = 2.0 * sqrt(2.0 * log(2.0)) * rmsdssigmacorrected * detector->scale;
    oaxaca_info("ao: dynamic Hartmann FWHM is %.2f arcsec.", hartmannfwhm);
    detector->hartmannfwhm = hartmannfwhm;
  }

  {
    double sumsq = 0.0;
    for (int i = 0; i < ns; ++i) {
      sumsq += pow(ds1[i] / ds0[0], 2);
    }
    double rmsds = sqrt(sumsq / ns);
    oaxaca_debug("ao: rmsds = %.3f pixels (raw)", rmsds);
    double hartmannfwhm = 2.0 * sqrt(2.0 * log(2.0)) * rmsds * detector->scale;
    oaxaca_info("ao: static Hartmann FWHM is %.2f arcsec.", hartmannfwhm);
    detector->hartmannfwhm = hartmannfwhm;
  }
  
  {
    double sumsq = 0.0;
    for (int i = 0; i < ns; ++i) {
      sumsq += ds2[i] / ds0[0];
    }
    double rmsds = sqrt(sumsq / ns);
    oaxaca_debug("ao: rmsds = %.3f pixels (raw)", rmsds);
    double rmsdsnoise = 57.294 * pow(detector->eshm, -0.8748);
    oaxaca_debug("ao: rmsds = %.3f pixels (noise)", rmsdsnoise);
    double rmsdscorrected = 
      (rmsdsnoise > rmsds) ? 0.0 : sqrt(rmsds * rmsds - rmsdsnoise * rmsdsnoise);
    oaxaca_debug("ao: rmsds = %.3f pixels (corrected)", rmsdscorrected);
    double hartmannfwhm = 2.0 * sqrt(2.0 * log(2.0)) * rmsdscorrected * detector->scale;
    oaxaca_info("ao: total Hartmann FWHM is %.2f arcsec.", hartmannfwhm);
    detector->hartmannfwhm = hartmannfwhm;
  }
  
  if (detector->totaldelay != 0.0) {
    double meandelay = detector->totaldelay / detector->framesperintegration;
    oaxaca_info("ao: mean detector delay is %.0f µs.", meandelay * 1e6);
    detector->totaldelay = 0.0;
  }
  if (oaxaca_shm->wfc.totaldelay != 0) {
    double meandelay = oaxaca_shm->wfc.totaldelay / detector->framesperintegration;
    oaxaca_info("ao: mean wfc delay is %.0f µs.", meandelay * 1e6);
    oaxaca_shm->wfc.totaldelay = 0.0;
  }
  
  ao_log(index);

  for (int i = 0; i < OAXACA_NS_MAX; ++i)
    ds0[i] = ds1[i] = ds2[i] = 0.0;
}
